#include <ros/ros.h>
#include "common_lib/common.h"

using namespace common_lib;

void Common::SplitString(const std::string &s, std::vector<std::string> &v, const std::string &c)
{
    std::string::size_type pos1, pos2;
    pos2 = s.find(c);
    pos1 = 0;
    while (std::string::npos != pos2)
    {
        v.push_back(s.substr(pos1, pos2 - pos1));

        pos1 = pos2 + c.size();
        pos2 = s.find(c, pos1);
    }

    if (pos1 != s.length())
    {
        v.push_back(s.substr(pos1));
    }
}

float Common::AngleConstraintToPI(float angle) const
{
    return atan2(sin(angle), cos(angle));
}

float Common::Degree2Radian(float degree_value)
{
    return degree_value * M_PI / 180.0f;
}
float Common::Radian2Degree(float radian_value)
{
    return radian_value * 180.0f / M_PI;
}

double Common::ValueConstraintMinToMax(double value, double min, double max)
{
    if (value > max)
    {
        value = max;
        return value;
    }
    else if (value < min)
    {
        value = min;
        return min;
    }
    return value;
}

float Common::Point2LineDistanceWithSign(float p_x, float p_y, float l_x, float l_y, float l_theta)
{
    // 直线方程 cos(l_theta)*x - sin(l_theta)*y + sin(l_theta)*l_y - cos(l_theta)*l_x = 0
    //  if (sin(l_theta) < 0)
    //  	return -(sin(l_theta) * p_x - cos(l_theta) * p_y - sin(l_theta) * l_x + cos(l_theta) * l_y);
    //  else
    //  	return
    //  sin(l_theta) * p_x - cos(l_theta) * p_y - sin(l_theta) * l_x + cos(l_theta) * l_y;
    float x1 = cos(l_theta);
    float y1 = sin(l_theta);
    float x2 = l_x - p_x;
    float y2 = l_y - p_y;
    float sign;
    if (x1 * y2 - x2 * y1 > 0)
        sign = 1;
    else
        sign = -1;
    return sign * fabs(y1 * p_x - x1 * p_y - y1 * l_x + x1 * l_y);
}

bool Common::Point2LineProjection(float p_x, float p_y, float begin_x, float begin_y, float end_x, float end_y, float &pj_x, float &pj_y)
{
    double dx = begin_x - end_x;
    double dy = begin_y - end_y;

    if (fabs(dx) < FLT_EPSILON && fabs(dy) < FLT_EPSILON)
    {
        return false;
    }
    double k = (p_x - begin_x) * (begin_x - end_x) + (p_y - begin_y) * (begin_y - end_y);
    k = k / (pow(dx, 2) + pow(dy, 2));
    pj_x = begin_x + k * dx;
    pj_y = begin_y + k * dy;
    return true;
}

bool Common::PointInLineSegment(float p_x, float p_y, float begin_x, float begin_y, float end_x, float end_y)
{
    if((p_x - begin_x) * (end_y - begin_y) == (end_x - begin_x) * (p_y - begin_y)  //叉乘 
       && (std::min(begin_x , end_x) - p_x <= 0.0005f) && (p_x - std::max(begin_x , end_x) <= 0.0005f )    //保证点坐标在begin,end之间 
       && (std::min(begin_y , end_y) - p_y <= 0.0005f) && (p_y - std::max(begin_y , end_y) <= 0.0005f))
        return true;
    else
        return false;
}

float Common::SectionLinearFcn(std::map<float,float> point,float x)
{
    // float temp;
    std::vector<float> v;
    for(auto p : point)
    {
        v.push_back(p.first);
    }
    std::sort(v.begin(),v.end());
    if(x < *v.begin())
        return point[*v.begin()];
    else if (x > *v.rbegin())
        return point[*v.rbegin()];
    else 
    {
        std::vector<float>::iterator it = v.begin();
        while(it != v.end())
        {
            if(x >= *it && x < *(it + 1))
                return point[*it] + (point[*(it + 1)] - point[*it])/(*(it + 1) - *it)*(x - *it);
            it ++;
        }
    }
    return 0.0f;
}

double Common::getVectorAngle2d(const double angle, const float x, const float y)
{
	double theta = angle - atan2(y, x); //弧度
	if (theta > M_PI)
	{
		theta -= 2*M_PI;
	}
	if (theta < -M_PI)
	{
		theta += 2*M_PI;
	}
	return theta;// * 180/M_PI; //角度
}

double Common::getVectorAngle2d(const double x1, const double y1, const double x2, const double y2)
{
    double theta = atan2(y1, x1);
    return getVectorAngle2d(theta, x2, y2);
}

DoubleThreshold_e DoubleThreshold::GetState(float value)
{
    if (value > up_threshold_)
    {
        last_state_ = DoubleThreshold_e::UP_AREA;
        // std::cout<<"last_state_:"<<last_state_<<std::endl;
        return DoubleThreshold_e::UP_AREA;
    }
    else if (value < down_threshold_)
    {

        last_state_ = DoubleThreshold_e::DOEN_AREA;
        // std::cout<<"last_state_:"<<last_state_<<std::endl;
        return DoubleThreshold_e::DOEN_AREA;
    }
    else
    {
        // std::cout<<"last_state_:"<<last_state_<<std::endl;
        return last_state_;
    }
}
void DoubleThreshold::SetDownThreshold(const float &down_threshold)
{
    down_threshold_ = down_threshold;
}
void DoubleThreshold::SetUpThreshold(const float &up_threshold)
{
    up_threshold_ = up_threshold;
}

TON::TON()
{
    is_first_ = true;
};
bool TON::Q(bool state, float delay_time)
{
    if (is_first_)
    {
        start_time_ = ros::Time::now().toSec();
        is_first_ = false;
    }
    if (!state)
    {
        start_time_ = ros::Time::now().toSec();
    }
    if (ros::Time::now().toSec() - start_time_ > delay_time * 0.001 && state)
    {
        return true;
    }
    else
    {
        return false;
    }
}
TOF::TOF()
{
    is_first_ = true;
};
bool TOF::Q(bool state, float delay_time)
{
    if (is_first_)
    {
        start_time_ = ros::Time::now().toSec();
        is_first_ = false;
    }
    if (state)
    {
        start_time_ = ros::Time::now().toSec();
    }
    if (ros::Time::now().toSec() - start_time_ > delay_time * 0.001 && !state)
    {
        return false;
    }
    else
    {
        return true;
    }
}

SpeedSmoothWithCloseConstAcc::SpeedSmoothWithCloseConstAcc(float period_s)
    : period_s_(period_s)
{
    last_output_ = 0.0;
    target_ = 0.0;
    output_ = 0.0;
}

SpeedSmoothWithCloseConstAcc::~SpeedSmoothWithCloseConstAcc()
{
}

float SpeedSmoothWithCloseConstAcc::VelocityConfig(float target_speed, float current_speed, float acc, float dcc)
{
    if (fabs(target_ - target_speed) > 0.01)
    {
        target_ = target_speed;
        last_output_ = current_speed;
    }

    // close loop: use the current_speed as the base
    last_output_ = current_speed;

    float det_speed = target_speed - last_output_;
    float det_acc = acc * period_s_;
    float det_dcc = dcc * period_s_;

    float direction = (current_speed >= 0) - (current_speed < 0);

    float output_increase = last_output_ + det_acc * direction;
    float output_decrease = last_output_ - det_dcc * direction;

    if (fabs(target_ - output_increase) > fabs(target_ - output_decrease))
    {
        if (fabs(det_speed) < fabs(det_dcc))
        {
            output_ = target_speed;
        }
        else
        {
            output_ = output_decrease;
        }
    }
    else
    {
        if (fabs(det_speed) < fabs(det_acc))
        {
            output_ = target_speed;
        }
        else
        {
            output_ = output_increase;
        }
    }

    last_output_ = output_;
    return output_;
}
float SpeedSmoothWithCloseConstAcc::VelocityConfigToEnd(float end_dis, float current_speed, float acc, float dcc)
{
    float reason_speed = sqrt(2.0 * fabs(dcc * end_dis));
    return reason_speed;
}
void SpeedSmoothWithCloseConstAcc::Reset()
{
    last_output_ = 0.0;
    target_ = 0.0;
    output_ = 0.0;
}
